
#include "PinJoint.h"

namespace p2 {

IMPLEMENT_RTT(p2, PinJoint);

PinJoint::~PinJoint()
{
}

PinJoint* PinJoint::create(Anchor const& a0, Anchor const& a1, real length, void* userData)
{
	PinJoint* newPinJoint = _allocator.allocate(1);
	if (newPinJoint) {
		_allocator.construct(newPinJoint, PinJoint(a0, a1, length, userData));
	}
	return newPinJoint;
}

void PinJoint::preStep(real64 dT)
{
	Constraint::preStep(dT);

    Entity const* e0 = _anchor0.getEntity();
	Entity const* e1 = _anchor1.getEntity();

	if (_flags & (FLAGS_ENABLE_MOTOR | FLAGS_ENABLE_FRICTION | FLAGS_ENABLE_LIMITS)) {
		_mA = (e0 ? e0->getInvInertia() : 0.f) + (e1 ? e1->getInvInertia() : 0.f);
		if (_mA > 0.0f) {
			_mA = 1.0f / _mA;
		}
	}
}

bool PinJoint::solvePositions(real64 dT)
{
	computeFutureAnchorPositions(dT);

    Entity const* e0 = _anchor0.getEntity();
	Entity const* e1 = _anchor1.getEntity();

	if (_flags & FLAGS_ENABLE_LIMITS) {
		/*
		 * apply angular impulse to correct if angular difference falls outside limits
		 */
		real da = (e1 ? e1->getA() : 0.f) - (e0 ? e0->getA() : 0.f);
		real dal = (da < _minLimit ? _minLimit - da : 0.f) + (da > _maxLimit ? _maxLimit - da : 0.f);
		dal = +Math<real>::Clamp(dal, -MAX_ANGULAR_CORRECTION, +MAX_ANGULAR_CORRECTION);
		real limitingImpulse = _mA * dal / (real)dT;
		if ( e0 ) {
			_dv[0].incW(-e0->getInvInertia() * limitingImpulse);
		}
		if ( e1 ) {
			_dv[1].incW(+e1->getInvInertia() * limitingImpulse);
		}
	}
    /*
     * REF: http://i31www.ira.uka.de/docs/Paper_VC05.pdf
     * 'Now the distance vector d between the two points at time t0 + h can be determined:
     * d(t0 + h) = B(t0 + h) - A(t0 + h)'
     */
	/*
	 * work out the length to correct and check if it needs to be corrected
	 */
    real dth = _length - _duth;
    real dxth = Math<real>::Clamp(dth, -MAX_DISTANCE_CORRECTION, (_flags & FLAGS_FLEXIBLE_LENGTH)?0.f:+MAX_DISTANCE_CORRECTION);
    if ( Math<real>::FAbs(dth) <= EPOS ) {
        return true;
    }
    _uth /= _duth;

    _invK = Entity::invK(e0, e1, _rAth, _rBth, _uth);

    Vector2<real> impulse = Vector2<real>::ZERO;
    if (_flags & FLAGS_ENABLE_SPRING) {
        /*
         * REF: http://gafferongames.com/game-physics/spring-physics/
         * 'A spring-damper system can be modeled as follows: F = - kx - bv
         * where b is the coefficient of damping and v is the relative velocity
         * between the two points connected by the spring'
         */
        Vector2<real> dv((e1 ? (e1->getV()+_dv[1].getV()) + (e1->getW()+_dv[1].getW()) * _rBth.perp() : Vector2<real>::ZERO)
        		- (e0 ? (e0->getV()+_dv[0].getV()) + (e0->getW()+_dv[0].getW()) * _rAth.perp() : Vector2<real>::ZERO));
        real dvth = -_uth.dot(dv);
        real F = - _k * dxth - _b * dvth;
        real mag = F * (real)dT * _invK;
        impulse = mag * _uth;
    } else {
		/*
		 * REF: http://i31www.ira.uka.de/docs/Paper_VC05.pdf
		 * 'An impulse P that eliminates the distance d in a time step of length h must change
		 * the relative velocity of the two points by d / h.'
		 */
		real mag = -dxth / (real)dT * _invK;
		impulse = mag * _uth;
    }
	/*
	 * apply impulses
	 */
	if ( e0 ) {
		_dv[0].incV(+e0->getInvMass() * impulse);
		_dv[0].incW(+e0->getInvInertia() * _rAth.perp().dot(impulse));
    }
    if ( e1 ) {
    	_dv[1].incV(-e1->getInvMass() * impulse);
    	_dv[1].incW(-e1->getInvInertia() * _rBth.perp().dot(impulse));
    }
    return false;
}

bool PinJoint::solveVelocities(real64 dT)
{
    Entity const* e0 = _anchor0.getEntity();
	Entity const* e1 = _anchor1.getEntity();

	if (_flags & (FLAGS_ENABLE_MOTOR | FLAGS_ENABLE_FRICTION)) {
		/*
		 * apply friction or motor impulse
		 */
		real dw = (e1 ? (e1->getW()+_dv[1].getW()) : 0.f) - (e0 ? (e0->getW()+_dv[0].getW()) : 0.f);
		real effectImpulse = 0;
		if (_flags & FLAGS_ENABLE_MOTOR) {
			/*
			 * implement motor with specified angular velocity
			 */
			effectImpulse += _mA * +Math<real>::Clamp(_motorW - dw, -MAX_ANGULAR_CORRECTION, +MAX_ANGULAR_CORRECTION);
			/*
			 * implement motor with specified torque
			 */
			effectImpulse += _motorTorque * (real)dT;
		}
		if (_flags & FLAGS_ENABLE_FRICTION) {
			/*
			 * implement friction as scaled mass and negative velocity product
			 * as we will loose a scaled proportion of angular impulse through friction
			 */
			effectImpulse += _mA * _muA * -Math<real>::Clamp(dw, -MAX_ANGULAR_CORRECTION, +MAX_ANGULAR_CORRECTION);
		}
		if ( e0 ) {
			_dv[0].incW(-e0->getInvInertia() * effectImpulse);
		}
		if ( e1 ) {
			_dv[1].incW(+e1->getInvInertia() * effectImpulse);
		}
	}
	/*
	 * only correct the velocity difference if it is a joint with no length
	 */
	if (_length != 0.f) {
		return false;
	}
    /*
     * REF: http://i31www.ira.uka.de/docs/Paper_VC05.pdf
     * 'With the velocity difference dut = (uBt - uAt) and the matrix
     * K = (Ka + Kb) from the last section an impulse P can
     * be computed which eliminates the velocity difference if it is
     * applied to the points in opposite directions'
     */
    Vector2<real> dv = (e1 ? (e1->getV()+_dv[1].getV()) + (e1->getW()+_dv[1].getW()) * _rBth.perp() : Vector2<real>::ZERO)
     		- (e0 ? (e0->getV()+_dv[0].getV()) + (e0->getW()+_dv[0].getW()) * _rAth.perp() : Vector2<real>::ZERO);
    real dvth = dv.dot(_uth);
    if ( Math<real>::FAbs(dvth) <= EVEL ) {
        return true;
    }

	/*
	 * eliminate velocity difference dut
	 */
    real remove = Math<real>::Clamp(dvth, -MAX_VELOCITY_CORRECTION, +MAX_VELOCITY_CORRECTION);
    real mag = remove * _invK;
    Vector2<real> impulse = mag * _uth;
    /*
	 * apply impulses
	 */
	if ( e0 ) {
		_dv[0].incV(+e0->getInvMass() * impulse);
		_dv[0].incW(+e0->getInvInertia() * _rAth.perp().dot(impulse));
    }
    if ( e1 ) {
    	_dv[1].incV(-e1->getInvMass() * impulse);
    	_dv[1].incW(-e1->getInvInertia() * _rBth.perp().dot(impulse));
    }
    return false;
}

}
